# -*- coding: utf-8 -*-
import threading
import time
import socket
import rospy
import moveit_msgs.msg
from transform import *
from ti5_kinematic import Ti5Kinematic
#from Speed.msg import Speed   # 将订阅str_message话题，自定义消息类型Speed::Speed
from std_msgs.msg import String
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
Vc=-1 #需要在ROS和socket中传递或使用的变量设为全局变量
class Server(object):
    def __init__(self):
        rospy.init_node('position_pubscriber', anonymous=True)   # ROS节点初始化
        self.pub = rospy.Publisher('ik_result', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
        self.arm_kinematic = Ti5Kinematic()
        self.last_position=[0,0,0,0,0,0]

        group_name = "armgroup"
        self.group = moveit_commander.MoveGroupCommander(group_name)

    def InfoCallback(self,msg):#ROS订阅的回调函数
        global Vc
        Vc=msg.carspeed   #订阅本车的速度

    def send_info(self,client): #SOCKET的发送函数
        global Vc
        while(1):
            if Vc>=0:
                hvjson=Vc #将vc发送出去
                client.sendall(hvjson.encode("utf-8"))
                time.sleep(1)

    def list_get_min(self,li):
        each_cha = 0
        for i in range(6):
            sta = 0
            if li[i]>0 and self.last_position[i]>0:
                sta=1
            if li[i]<0 and self.last_position[i]<0:
                sta=1
            if sta==1:
                each_cha += abs(self.last_position[i]-li[i])
            elif sta==0:
                each_cha += abs(self.last_position[i])+abs(li[i])
            #print(str(each_cha))
        return each_cha

    def recv_info(self,client): #SOCKET的接收函数
        global data
        while(1):
            #try:
            if 1>0:
                end_point=[0,0,0,0,0,0]

                info=client.recv(2000)
                data=info.decode("utf-8")   #接收到的数据
                #self.pub.publish(data)  #send to rostopic  <<charter>>
                if data[0]=="r":
                    data = data[1:]
                    li_1 = data.split(",")
                    li_2 = li_1[0].split(" ")
                    li = map(float,li_2)
                    end_point=li
                    print("end_point"+str(end_point))
                elif data[0]=="j":
                    data = data[1:]
                    li_1=data.split(",")
                    li_2 = li_1[0].split(" ")
                    li = map(float,li_2)
                    target_pose = self.arm_kinematic.forward_kinematic_v2(li)
                    #candi_joint_angle_list = self.arm_kinematic.inverse_kinematic(target_pose)
                    
                    end_point[0]=target_pose.x
                    end_point[1]=target_pose.y
                    end_point[2]=target_pose.z
                    end_point[3]=target_pose.roll
                    end_point[4]=target_pose.pitch
                    end_point[5]=target_pose.yaw
                    print("end_point"+str(end_point))


                if end_point:
                    pose_target= geometry_msgs.msg.Pose()
                    pose_target.position.x=end_point[0]
                    pose_target.position.y=end_point[1]
                    pose_target.position.z=end_point[2]
                    pose_target.orientation.x=end_point[3]
                    pose_target.orientation.y=end_point[4]
                    pose_target.orientation.z=end_point[5]
                    pose_target.orientation.w=0.479

                    #get start point
                    curr_p = self.group.get_current_pose().pose.position
                    curr_rpy = self.group.get_current_rpy()
                    start_point=[curr_p.x,curr_p.y,curr_p.z,curr_rpy[0],curr_rpy[1],curr_rpy[2]]
                    print("starting point:",str(start_point))


                    #plan  = self.group.plan(pose_target)
                    #plan_points_list = plan.joint_trajectory.points
"""
                    for li in plan_points_list:
                        target_pose = self.arm_kinematic.forward_kinematic_v2(li)
                        candi_joint_angle_list_f = self.arm_kinematic.inverse_kinematic(target_pose)

                    if candi_joint_angle_list:
                        cha=10
                        res_select=0 #the result min total_cha list's number
                        numnum = 0
                        for t in candi_joint_angle_list_f:
                            now_cha = self.list_get_min(t)
                            if now_cha<cha:
                                cha=now_cha
                                res_select = numnum
                            numnum+=1

            #except:
            #    break

"""


if __name__ == '__main__':
#先开启了ROS
    ser = Server()
    #rospy.Subscriber("str_message", Speed, InfoCallback)   # 创建一个Subscriber，订阅名为/str_message的topic，注册回调函数InfoCallback
 
 #再创建了SOCKET       
    g_socket_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  #创建socket
    g_socket_server.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,1)
    g_socket_server.bind(("", 1436))     #关联相应的地址和端口号
    g_socket_server.listen(128)          #开始监听
    client, _ = g_socket_server.accept()    #接收并建立与客户端的连接，程序在此处开始阻塞，只有等到客户端连接进来开始往下执行
    threads = []
    t1 = threading.Thread(target=ser.send_info, args=(client,))  #开启线程
    threads.append(t1)
    t2 = threading.Thread(target=ser.recv_info, args=(client,))
    threads.append(t2)
    for t in threads:
        print "1212121"
        t.start()
        time.sleep(1)


